/*
 * To change this license header, choose License Headers in Project Properties.
 * To change this template file, choose Tools | Templates
 * and open the template in the editor.
 */
package edu.wpi.first.wpilibj.templates.subsystems;

import edu.wpi.first.wpilibj.command.Subsystem;
import edu.wpi.first.wpilibj.Gyro;
import edu.wpi.first.wpilibj.templates.RobotMap;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj.templates.commands.GetAngle;

/**
 *
 * @author Robotics
 */
public class Gyroscope extends Subsystem {
    // Put methods for controlling this subsystem
    // here. Call these from Commands.
    private Gyro gyro;
    
    public Gyroscope(){
        
        gyro = new Gyro(RobotMap.GYRO_AC);
        //Gyro.setSensitivity(SmartDashboard.getNumber("gyro Sensitivity: ", 0) / 1000.0);
        gyro.setSensitivity(0.007);

        gyro.reset();
        SmartDashboard.putNumber("Gyro Angle: ", gyro.getAngle());
    }
    
    public double getAngle(){
        return gyro.getAngle();
    }
    
    public void setSensitivity(double Sensitivity){
        gyro.setSensitivity(Sensitivity);
    }
    
    public void resetGyro(){
        gyro.reset();
    }
    
    public void initDefaultCommand() {
        // Set the default command for a subsystem here.
        //setDefaultCommand(new MySpecialCommand());
        setDefaultCommand(new GetAngle());
    }
}
